﻿using OnDash.Mathematics.Vector;

namespace OnDash.Mathematics.Geometry;

public struct BezierCurveCubic(
    float parallel,
    Vector2 startAnchor,
    Vector2 endAnchor,
    Vector2 firstControlPoint,
    Vector2 secondControlPoint)
{
    private Vector2 StartAnchor = startAnchor;
    private Vector2 EndAnchor = endAnchor;
    private Vector2 FirstControlPoint = firstControlPoint;
    private Vector2 SecondControlPoint = secondControlPoint;
    private readonly float Parallel = parallel;

    public BezierCurveCubic(Vector2 startAnchor, Vector2 endAnchor, Vector2 firstControlPoint,
        Vector2 secondControlPoint) : this(0f, startAnchor, endAnchor, firstControlPoint, secondControlPoint)
    {
    }

    public Vector2 CalculatePoint(float t)
    {
        var r = new Vector2();
        var c = 1f - t;

        r.X = StartAnchor.X * c * c * c + FirstControlPoint.X * 3 * t * c * c +
              SecondControlPoint.X * 3 * t * t * c
              + EndAnchor.X * t * t * t;

        r.Y = StartAnchor.Y * c * c * c + FirstControlPoint.Y * 3 * t * c * c +
              SecondControlPoint.Y * 3 * t * t * c
              + EndAnchor.Y * t * t * t;

        if (Parallel == 0f) return r;

        Vector2 perpendicular;

        if (t == 0f) perpendicular = FirstControlPoint - StartAnchor;
        else perpendicular = r - CalculatePointOfDerivative(t);

        return r + Vector2.Normalize(perpendicular).PerpendicularRight * Parallel;
    }

    private Vector2 CalculatePointOfDerivative(float t)
    {
        var r = new Vector2();
        var c = 1f - t;

        r.X = c * c * StartAnchor.X + 2 * t * c * FirstControlPoint.X + t * t * SecondControlPoint.X;
        r.Y = c * c * StartAnchor.Y + 2 * t * c * FirstControlPoint.Y + t * t * SecondControlPoint.Y;

        return r;
    }

    public float CalculateLength(float precision)
    {
        var length = 0f;
        var old = CalculatePoint(0f);

        for (var i = precision; i < 1f + precision; i += precision)
        {
            var n = CalculatePoint(i);
            length += (n - old).Length;
            old = n;
        }

        return length;
    }
}